/*
 * plane control system
 *
 * main
 *
 */

#include<stdio.h>
#include<pthread.h>
#include<unistd.h>
#include<sys/types.h>
#include<signal.h>


#include<gpiox.h>
#include<us100.h>
#include<wflyctrl.h>
#include<global_header.h>
//#include<PlaneNavigation.h>
#include<C_CamDet.h>

#include"mainctrl.h"
#include"start.h"

#define MAINCTRL_THREADS_NUM 3
pthread_t mainctrl_tid_ary[MAINCTRL_THREADS_NUM];


static char buf[BUFSIZ];
static CamPos camPos;
static pthread_mutex_t campos_mlock;

void *campos_refresh(void *arg) {
    //updatePosition(&camPos, &campos_mlock);
    return (NULL);
}

void *manipulate(void *arg) {
    /*
     * to control the plane lanch,
     * fly to the destination,
     * put the pingpong_ball into the color-box,
     * and stop stably.
     */

#define CamPosAllowNotInitTimes (4)
    int ret,
        i, /* common counter */
        wfly_fb_val,
        wfly_lr_val,
        plane_pos_x_old,
        plane_pos_x_new,
        plane_pos_y_old,
        plane_pos_y_new,
        CamPosNotInitStack = CamPosAllowNotInitTimes,
        isCamPosInit = 0;
    char user_input_char;

#if 0    /* cli-hand-shake to comfirm the `init plane routine` */
    do {
        printf("Press 'c' and 'Enter' to lanch.");
        user_input_char = getchar();
        setbuf(stdin, buf);
    } while(user_input_char != 'c');
#endif

    printf("launch!!!\n");
    ret = wfly_lauch();

    WaitMutexCP(campos_mlock);
    isCamPosInit = camPos.isInitDet; // Is init mark detected?
    ExitMutexCP(campos_mlock);

    //if(!isCamPosInit) { printf("Camera Position is not initialize successfully.\n"); pthread_exit(NULL); }

    /* to solve the indigenous flying-offset */
#define indigenous_flying_offset_lr (10) /* left and right */
#define indigenous_flying_offset_fb (0)  /* forward and backward */
    wfly_fb_ctrl(indigenous_flying_offset_fb);
    wfly_lr_ctrl(indigenous_flying_offset_lr);
    wfly_accel_up();
    printf("accelerator will accelerate 5.5 second.\n");
    abs_usleep(5.6 * 1000 * 1000);

#if 1 /* crazy algorithm */
    wfly_accel_stay(); wfly_lr_ctrl(0);
#define FORWARD_SECOND (5)
#define BACKWARD_SECOND (3)
    /* forward */
    for(i = 0; i < FORWARD_SECOND; i++) {
        printf("I will fly forward %d second.\n", FORWARD_SECOND - i);
        wfly_fb_ctrl(12);
        abs_usleep(1 * 1000 * 1000);
    }

    /* balance and throw up */
    wfly_fb_ctrl(0); wfly_lr_ctrl(0);
    abs_usleep(1.0 * 1000 * 1000); ping_pong_box_open();
    abs_usleep(0.5 * 1000 * 1000); ping_pong_box_close();
    usleep(2.0 * 1000 * 1000);

    /* backword */
    wfly_fb_ctrl(-15);
    usleep(1.0 * 1000 * 1000);
    wfly_lr_ctrl(-4);
    for(i = 0; i < 1; i++) {
        printf("I will fly backward %d second.\n", BACKWARD_SECOND - i);
        wfly_fb_ctrl(0);
        usleep(1 * 1000 * 1000);
    }
#endif

#if 0 /* perfect algorithm */
    wfly_accel_stay(); wfly_fb_ctrl(0); wfly_lr_ctrl(0);
#define TIME_FAC (4)
#define FBLR_UNIT (8)
#define TICK_UNIT (2)     /* NUM tick per second */
#define STAY_TIMES_SECOND (10)
#define STAY_TIME_TICKS (TICK_UNIT * STAY_TIMES_SECOND)
    for(i = 0; i < STAY_TIME_TICKS; i++) {
        /* status-machine begin */
        WaitMutexCP(campos_mlock);
        isCamPosInit = camPos.isInitDet; // Is init mark detected?
        plane_pos_x_new = camPos.initPt.x;
        plane_pos_y_new = camPos.initPt.y;
        ExitMutexCP(campos_mlock);

#if 1 /* Y control */ /* 0 | backward << Y_STABLE : 190~290 << forward | 480 */
#define Y_MIN (190)
#define Y_MAX (290)
        if(((i + 1) % TIME_FAC) == 0) {
            if(Y_MIN < plane_pos_y_new && plane_pos_y_new < Y_MAX)
                wfly_fb_val = 0; /* It's stable */
            else if(plane_pos_y_new < Y_MIN) /* need to fly to forward */
                wfly_fb_val = FBLR_UNIT;
            else if(plane_pos_y_new > Y_MAX) /* need to fly to backward */
                wfly_fb_val = -FBLR_UNIT;
            wfly_fb_ctrl(wfly_fb_val);
        } else
            wfly_fb_ctrl(0);
#endif /* Y control */

#if 1 /* X control */ /* 0 | right << X_STABLE : 270~370 << left | 640 */
#define X_MIN (270)
#define X_MAX (370)
        if((i + 1) % TIME_FAC == 0) {
            if(X_MIN < plane_pos_x_new && plane_pos_x_new < X_MAX)
                wfly_lr_val = 0; /* X stable */
            else if(plane_pos_x_new < X_MIN) /* need to fly left */
                wfly_lr_val = -FBLR_UNIT;
            else if(plane_pos_x_new > X_MAX) /* need to fly right */
                wfly_lr_val = FBLR_UNIT;
            wfly_lr_ctrl(wfly_lr_val);
        } else
            wfly_lr_ctrl(0);

        if((i + 1) % TICK_UNIT == 0)
            printf("I will stay in the sky %d second.\n", (STAY_TIME_TICKS - i)/TICK_UNIT);

        abs_usleep((1.0 / TICK_UNIT) * 1000 * 1000);
        /* status-machine release */
        wfly_fb_ctrl(0); wfly_lr_ctrl(0);
    }
#endif /*  X control */

#endif /* perfect algorithm */

    test_plane_land_func();

    printf("stop.\n");
    ret = wfly_stop();
    return (NULL);
}
